#include "control.h"

PID_TYPE PID_ROL={0.02,0,0.5,  0,0,0,0,10000};//角度外环参数
PID_TYPE PID_PIT={0.01,0,0.5,  0,0,0,0,10000};
PID_TYPE PID_YAW={2,0,0,  0,0,0,0,200}; //10,3

PID_TYPE PID_ROL_R={2.6,0,0,  0,0,0,0,500};//角速度内环参数 3.5
PID_TYPE PID_PIT_R={2.6,0,0,  0,0,0,0,500};
PID_TYPE PID_YAW_R={6,0,0};

PID_TYPE PID_HIGH={4,0.004,0,  0,0,0,0,4000};//高度外环参数 1.8  单环：0.9,0.8
PID_TYPE PID_HIGH_R={0.3,0,0,};//加速度内环参数  0.8


float Moto_1=0.0f,Moto_2=0.0f,Moto_3=0.0f,Moto_4=0.0f;
float speedbase=0,highaim=15,yaw=0,yaw_aim=0,yaw_1,rol=0,pit=0,rol_aim,pit_aim;  
int Uav_Enable=0,high_flag=0;

void Control(void)
{
	
				if(PPM_Databuf[5]>800&&PPM_Databuf[5]<1200) Uav_Enable=0;//无人机上锁
				if(PPM_Databuf[5]>1800&&PPM_Databuf[5]<2200) Uav_Enable=1;//无人机解锁
				
				if(PPM_Databuf[4]>800&&PPM_Databuf[4]<1200) high_flag=0;//定高标志
				if(PPM_Databuf[4]>1800&&PPM_Databuf[4]<2200) high_flag=1;//
	
				speedbase=PPM_Databuf[2]-1010;//高度
				if(speedbase>550) speedbase=550;
				highaim = (PPM_Databuf[2]-1530)/2.5f;
				if(highaim<15) highaim=15;
				if(highaim>180) highaim=180;
	
	      yaw_1=yaw;
				yaw=(1508-PPM_Databuf[3])/10;//偏航
				if(fabs(yaw)<10) yaw=0;//避免误打
			  if(yaw>50) yaw=50;
				if(yaw<-50) yaw=-50;
				if(ms10==300) yaw_aim=Att_Angle.yaw;//初始目标偏航角
				yaw_aim+=0.008f*yaw;                 //偏航角转速
				if(yaw_aim>180) yaw_aim=-180;
				if(yaw_aim<-180) yaw_aim=180;//磁力计解算YAW角范围为-180到180
				if(yaw_1!=0&&yaw==0) {yaw_aim=Att_Angle.yaw;}
	
				rol=(1496-PPM_Databuf[1])/20;//前后
				if(fabs(rol)<2) rol=0; 
				//if(rol==0&&fabs(Att_Angle.rol)>4) rol_aim=-1*Att_Angle.rol; else rol_aim=rol;
				if(rol>25) rol=25;
				if(rol<-25) rol=-25;
				
				pit=(PPM_Databuf[0]-1523)/20;//左右
				if(fabs(pit)<2) pit=0; 
				//if(pit==0) pit_aim=-0.3*Att_Angle.pit; else pit_aim=pit;
				if(pit>25) pit=25; 
				if(pit<-25) pit=-25;
				//if(PPM_Databuf[1]==0||PPM_Databuf[0]==0) {rol=0;pit=0;}
			
		    PID_Rol(&PID_ROL,rol,Att_Angle.rol);//角度外环 
		    PID_Pit(&PID_PIT,pit,Att_Angle.pit);
				PID_Yaw(&PID_YAW,yaw_aim,Att_Angle.yaw);
				
				PID_Angle_R(&PID_ROL_R,PID_ROL.OutPut,Gyr_rad.X*RadtoDeg);//角速度内环 
		    PID_Angle_R(&PID_PIT_R,PID_PIT.OutPut,Gyr_rad.Y*RadtoDeg); 
				PID_Angle_R(&PID_YAW_R,PID_YAW.OutPut,Gyr_rad.Z*RadtoDeg);
				
				PID_High(&PID_HIGH,highaim,Altitude);//高度外环
		    PID_High_R(&PID_HIGH_R,PID_HIGH.OutPut,V_Z);
				
				if(Uav_Enable&&speedbase>30&&ms10>300) 
				{
					Moto_1 = 1000 + speedbase + PID_HIGH_R.OutPut + PID_ROL_R.OutPut - PID_PIT_R.OutPut - PID_YAW_R.OutPut ;
					Moto_2 = 1000 + speedbase + PID_HIGH_R.OutPut - PID_ROL_R.OutPut - PID_PIT_R.OutPut + PID_YAW_R.OutPut ;
					Moto_3 = 1000 + speedbase + PID_HIGH_R.OutPut - PID_ROL_R.OutPut + PID_PIT_R.OutPut - PID_YAW_R.OutPut +22;
					Moto_4 = 1000 + speedbase + PID_HIGH_R.OutPut + PID_ROL_R.OutPut + PID_PIT_R.OutPut + PID_YAW_R.OutPut ;
				}
				else {Moto_1=1000;Moto_2=1000;Moto_3=1000;Moto_4=1000;}
				Safety_Check();//飞机失控检测
				
	     Moto_Pwm(Moto_1,Moto_2,Moto_3,Moto_4); //将此数值分配到定时器，输出对应占空比的PWM波
}
/******************************************************************************************
*函  数：void Safety_Check(void)
*功  能：飞机姿态安全监测
*参  数：无
*返回值：无
*备  注：如果飞机角度和加速度异常就将飞机上锁并停止电机，防止电机狂转打坏桨叶
*******************************************************************************************/
void Safety_Check(void)
{
  if((fabs(Att_Angle.pit)>35.0f||fabs(Att_Angle.rol)>35.0f) && (fabs(Acc_filt.X)>5.0f||fabs(Acc_filt.Y)>5.0f))
	{
		Moto_1=1000;
		Moto_2=1000;
		Moto_3=1000;
		Moto_4=1000;
	}
}
